#include <cstdlib>
#include <iostream>
#include <libobsensor/hpp/Error.hpp>
#include <memory>
#include <opencv2/opencv.hpp>
 
#include "camera.h"

cv::Mat cameraMatrix = (cv::Mat_<float>(3, 3) <<
    307.10092,   0.     , 314.4606 ,
      0.     , 307.11747, 199.90106,
      0.     ,   0.     ,   1.     );
cv::Mat distCoeffs = (cv::Mat_<float>(5, 1) << -0.019218, 0.012827, -0.000142, -0.000708, 0.000000);

int main(void) {
    try {
        Camera camera(true, false, false);
        camera.start();
        cv::Mat frameCalibration;
        while (cv::waitKey(1) != 27) { // ESC的ASCII码是27，按ESC键可以终止程序
            auto frame_set = camera.get();
            if (frame_set != nullptr) {
                cv::Mat img = camera.frame2mat(frame_set->colorFrame());
                if (!img.empty()) {
                    undistort(img, frameCalibration, cameraMatrix, distCoeffs);
                    cv::imshow("Original_image",img);
                    cv::imshow("Calibrated_image", frameCalibration);
                }
                img = camera.frame2mat(frame_set->irFrame());
                if (!img.empty()) {
                    cv::imshow("IR", img);
                }
                img = camera.frame2mat(frame_set->depthFrame());
                if (!img.empty()) {
                    cv::imshow("Depth", img);
                }
            }
        }
        camera.stop();
        cv::destroyAllWindows();
    } catch (const ob::Error &e) {
        std::cerr << "Function:" << e.getName() << "\nargs:" << e.getArgs()
                  << "\nmessage:" << e.getMessage() << "\ntype:" << e.getExceptionType()
                  << std::endl;
        exit(EXIT_FAILURE);
    } catch (const std::exception &e) {
        std::cerr << e.what() << std::endl;
        exit(EXIT_FAILURE);
    } catch (...) {
        std::cerr << "Unexpected Error!" << std::endl;
        exit(EXIT_FAILURE);
    }
    return EXIT_SUCCESS;
}